#include "robotcommands.h"

/************************************************************/
/*                 ROBOT COMMAND GROUP CLASS                */
/************************************************************/
RobotCommandGroup::RobotCommandGroup(Robot* robot) : RobotCommand(robot)
{}

void RobotCommandGroup::execute(){
    int i;

    //Tell the robot that this is a batch of commands
    //This will prevent signals from being fired at every
    //command.  Rather, they will be emitted after the
    //batch ends.
    _robot->startBatch();

    //Iterate through the the command list and execute them
    for(i = 0; i < _commandList.length(); i++){
        _commandList.at(i)->execute();
    }

    //Tell the robot that the command batch is done
    _robot->endBatch();
}

void RobotCommandGroup::undo(){
    int i;

    _robot->startBatch();

    for(i = _commandList.length() - 1; i >= 0; i--){
        _commandList.at(i)->undo();
    }

    _robot->endBatch();
}

void RobotCommandGroup::append(RobotCommand *command){
    //Insert a new command into the list
    _commandList.append(command);
}


/************************************************************/
/*             MOVE ROBOT JOINT COMMAND CLASS               */
/************************************************************/
MoveRobotJointCommand::MoveRobotJointCommand(
        Robot* robot, int jointID, float delta) :
    RobotCommand(robot),
    _jointID(jointID),
    _delta(delta)
{
    //Save the previous state of the joint for undo
    _prevState = robot->jointAngle(jointID);
}

void MoveRobotJointCommand::execute(){
    _robot->moveJointBy(_jointID, _delta);
}

void MoveRobotJointCommand::undo(){
    _robot->setJointAngle(_jointID, _prevState);
}


/************************************************************/
/*              SET ROBOT JOINT COMMAND CLASS               */
/************************************************************/
SetRobotJointCommand::SetRobotJointCommand(
        Robot* robot, int jointID, float theta) :
    RobotCommand(robot),
    _jointID(jointID),
    _theta(theta)
{
    //Save the previous state for undo
    _prevState = robot->jointAngle(jointID);
}

void SetRobotJointCommand::execute(){
    _robot->setJointAngle(_jointID, _theta);
}

void SetRobotJointCommand::undo(){
    _robot->setJointAngle(_jointID, _prevState);
}

